#include "tinyOS.h"
#include "ARMCM3.h"

#define NVIC_INT_CTRL		0xE000ED04	// 中断控制及状态寄存器
#define	NVIC_PENDSVSET		0x10000000	// 触发软件中断的值
#define NVIC_SYSPRI2		0xE000ED22	// 系统优先级寄存器
#define NVIC_PENDSV_PRI		0x000000FF	// 配置优先级


#define MEM32(addr)			*(volatile unsigned long *)(addr)
#define MEM8(addr)			*(volatile unsigned char *)(addr)

#if 0
__asm void PendSV_Handler()
{
    IMPORT blockPtr

    /* 加载寄存器存储地址 */
    LDR R0, = blockPtr      // R0 = blockPtr
    LDR R0, [R0]            // 将R0的地址中的数据赋值给R0, R0 = *R0
    LDR R0, [R0]            // 将R0的地址中的数据赋值给R0, R0 = *R0

    /* 保存寄存器 */
    STMDB R0!, {R4-R11}     // 存储R4-R11到R0地址处，每存一个字前R0自减一次

    /* 将最后的地址写入到blockPtr中 */
    LDR R1, = blockPtr      // R1 = blockPtr
    LDR R1, [R1]            // 将R1的地址中的数据赋值给R1, R1 = *R1
    STR R0, [R1]            // 将R0的数据赋值给地址R0, *R1 = R0

    /* 修改部分寄存器,用于测试 */
    ADD R4, R4, #1          // R4 += 1
    ADD R5, R5, #1          // R5 += 1

    /* 恢复寄存器 */
    LDMIA R0!, {R4-R11}     // 从R0处读取多个字节到R4-R11,每读一个字后R0自增一次


    /* 异常返回 */
    BX LR

}
#endif

__asm void PendSV_Handler()
{
    IMPORT currentTask
    IMPORT nextTask

    MRS R0, PSP                     // 获取当前任务的堆栈指针
    CBZ R0, PendSVHandler_nosave    // 如果R0=0,则跳转到PendSVHandler_nosave

    /* 当前任务数据 */
    STMDB R0!, {R4-R11}             // 往R0写入多个字，地址在每次写入前减小，并保存最后的R0地址

    LDR R1, = currentTask           // R1 = currentTask
    LDR R1, [R1]                    // R1 = *R1
    STR R0, [R1]                    // *R1 = R0

PendSVHandler_nosave

    /* 恢复下一个任务数据 */
    LDR R0, = currentTask           // R0 = currentTask
    LDR R1, = nextTask              // R1 = nextTask
    LDR R2, [R1]                    // R2 = *R1
    STR R2, [R0]                    // *R0 = R2

    LDR R0, [R2]                    // R0 = *R2
    LDMIA R0!, {R4-R11}             // 从R0读取多个字，地址在每次读取后增加，并保存最后的R0地址

    MSR PSP, R0                     // 将R0写入PSP寄存器
    ORR LR, LR, #0x04               // LE |= 0X04
    BX LR

}


void tTaskRunFirst(void)
{
    __set_PSP(0);

    MEM8(NVIC_SYSPRI2) = NVIC_PENDSV_PRI;
    MEM32(NVIC_INT_CTRL) = NVIC_PENDSVSET;
}

void tTaskSwitch(void)
{
    MEM32(NVIC_INT_CTRL) = NVIC_PENDSVSET;  /* 使能PendSV */
}






